
#include "camera_intrinsic/intrinsic_calibration.h"

IntrinsicCalibrator::IntrinsicCalibrator(const IntrinsicConfig& config) 
    : config_(config),
    configInfo_(ConfigInstance::instance().get_config())
{
    generateObjectPoints();
}

void IntrinsicCalibrator::generateObjectPoints() {
    std::vector<cv::Point3f> objp;
    for (int i = 0; i < config_.board_size.height; ++i) {
        for (int j = 0; j < config_.board_size.width; ++j) {
            objp.emplace_back(j * config_.square_size, 
                             i * config_.square_size, 0.0f);
        }
    }
    object_points_.push_back(objp);
}

bool IntrinsicCalibrator::processFrame(const cv::Mat& frame) {
    image_size_ = frame.size();

    std::vector<cv::Point2f> corners;
    if (!detectChessboard(frame, corners)) { // 检测棋盘格角点
        return false;
    }

    image_points_.push_back(corners);
    return true;
}

bool IntrinsicCalibrator::run() {

    std::vector<cv::String> image_files;
    cv::glob(pattern_, image_files); // 获取所有匹配的图像路径

    // 读取图片并提取角点
    for (size_t i = 0; i < image_files.size(); ++i) {
        cv::Mat img = cv::imread(image_files[i]); // 读取图像
        if (img.empty()) { // 检查图像是否为空
            std::cout << "无法读取图像: " << image_files[i] << std::endl;
            continue;
        }

        if (processFrame(img)) {
            std::cout << "成功处理帧: " << i << std::endl;
        } else {
            std::cout << "处理帧失败: " << i << std::endl;
        }
    }
    if (image_points_.size() < config_.min_frames) {
        std::cout << "标定图像数量不够，最少应有" << config_.min_frames << "张图像" << std::endl;
        return false;
    }

    // 准备多次使用的object points
    std::vector<std::vector<cv::Point3f>> object_points_array(
        image_points_.size(), object_points_[0]);

    // 配置标定参数
    int flags = config_.calibration_flags;
    if (!config_.use_radial_distortion) { // 如果不使用径向畸变校正
        flags |= cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 | cv::CALIB_FIX_K3; // 固定径向畸变系数K1、K2、K3
    }

    std::vector<cv::Mat> rvecs, tvecs; // 旋转，平移矩阵
    double error = cv::calibrateCamera(
        object_points_array,  // 3D点集
        image_points_,        // 2D点集
        image_size_,          // 图像尺寸
        camera_intr_matrix_,  // 输出相机内参矩阵
        dist_coeffs_,         // 输出畸变系数
        rvecs, tvecs,         // 输出每帧的外参（旋转向量和平移向量）
        flags                 // 标定标志
    );

    // 验证标定结果
    if (error < 0 || error > config_.max_reproj_error) { // 如果标定误差小于0或大于允许的最大误差
        std::cout << "标定失败，标定误差为：" << error << std::endl;
        return false;
    }

    reproj_error_ = error;

    // 标定完成后输出结果和保存矫正图像
    std::cout << "\n标定成功!" << std::endl;
    std::cout << "相机内参矩阵:\n" << camera_intr_matrix_ << std::endl;
    std::cout << "畸变系数:\n" << dist_coeffs_ << std::endl;
    std::cout << "平均重投影误差: " << reproj_error_ << "像素" << std::endl;

    // 保存标定结果
    if (!saveResults(camera_intr_params_path_)) {
        std::cout << "保存标定结果失败！" << std::endl;
        return false;
    };

    // // 加载标定结果
    // if (!loadResults(camera_intr_params_path_)) {
    //     std::cout << "加载标定结果失败！" << std::endl;
    //     return false;
    // };

    // 矫正并保存图像
    // saveUndistortedImages(output_dir_);
    return true;
}

bool IntrinsicCalibrator::detectChessboard(const cv::Mat& frame,
                                         std::vector<cv::Point2f>& corners) const {
    cv::Mat gray;
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); // 灰度

    bool found = cv::findChessboardCorners( // 角点检测
        gray, config_.board_size, corners,
        cv::CALIB_CB_ADAPTIVE_THRESH | // 自适应阈值
        cv::CALIB_CB_NORMALIZE_IMAGE | // img归一化
        cv::CALIB_CB_FAST_CHECK // 快速检测棋盘格是否存在
    );

    if (found) {
        // 亚像素精确化
        cv::cornerSubPix(
            gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
            cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 
                            30, 0.01)
        );
    } else {
        std::cout << "图像角点提取失败！" << std::endl;
        return false;
    }

    return true;
}

bool IntrinsicCalibrator::saveResults(const std::string& path) {
    std::cout << "开始保存标定结果..." << std::endl;
    cv::FileStorage fs(path, cv::FileStorage::WRITE);
    if (!fs.isOpened()) return false;

    fs << "camera_matrix" << camera_intr_matrix_
       << "dist_coeffs" << dist_coeffs_
       << "reprojection_error" << reproj_error_;
    return true;
}

bool IntrinsicCalibrator::loadResults(const std::string& path) {
    std::cout << "开始加载标定结果..." << std::endl;
    cv::FileStorage fs(path, cv::FileStorage::READ);
    if (!fs.isOpened()) return false;

    fs["camera_matrix"] >> camera_intr_matrix_;
    fs["dist_coeffs"] >> dist_coeffs_;
    fs["reprojection_error"] >> reproj_error_;
    return true;
}

bool IntrinsicCalibrator::saveUndistortedImages(std::string output_dir) {
    if (system(("mkdir -p " + output_dir).c_str()) != 0) {
        std::cout << "创建输出目录失败！" << std::endl;
        return false;
    }
    // 对每张图像进行畸变矫正
    std::vector<cv::String> image_files;
    cv::glob(pattern_, image_files); // 获取所有匹配的图像路径

    // 读取图片并提取角点
    for (size_t i = 0; i < image_files.size(); ++i) {
        cv::Mat img = cv::imread(image_files[i]);
        cv::Mat undistorted;
        cv::undistort(img, undistorted, camera_intr_matrix_, dist_coeffs_);
        
        // 构造输出文件名并保存结果
        std::string filename = output_dir + "undistorted_" + 
                                std::to_string(i) + ".png";
        cv::imwrite(filename, undistorted);
        std::cout << "已保存矫正后的图像: " << filename << std::endl;
    }
    return true;
}
